#include "uavmath.h"

const quaternion_t QUATERNION_ZERO = { { { 0LL }, { 0LL }, { 0LL } }, { 0LL };

fixed_t quaternion_length(const quaternion_t *q)
{
	fixed_t res = vector3_length_squared(&q->v);
	res.val += fixed_mul(q->w, q->w).val;
	return fixed_sqrt(res);
}

fixed_t quaternion_length_squared(const quaternion_t *q)
{
	fixed_t res = vector3_length_squared(&q->v);
	res.val += fixed_mul(q->w, q->w).val;
	return res;
}

void quaternion_normalize(const quaternion_t *q, quaternion_t *out)
{
	fixed_t len = quaternion_length(q);
	out->v.x = fixed_div(q->v.x, len);
	out->v.y = fixed_div(q->v.y, len);
	out->v.z = fixed_div(q->v.z, len);
	out->w = fixed_div(q->w, len);
}

void quaternion_conjugate(const quaternion_t *q, quaternion_t *out)
{
	out->v.x.val = -q->v.x.val;
	out->v.y.val = -q->v.y.val;
	out->v.z.val = -q->v.z.val;
	out->w.val =  q->w.val; // don't negate!
}

void quaternion_mul(const quaternion_t *q1, const quaternion_t *q2, quaternion_t *out)
{
	out->w.val = fixed_mul(q1->w, q2->w).val - vector3_dot(&q1->v, &q2->v);
	vector3_cross(&q1->v, &q2->v, &out->v);
	out->v.x.val += fixed_mul(q1->w, q2->v.x).val + fixed_mul(q2->w, q1->v.x).val;
	out->v.y.val += fixed_mul(q1->w, q2->v.y).val + fixed_mul(q2->w, q1->v.y).val;
	out->v.z.val += fixed_mul(q1->w, q2->v.z).val + fixed_mul(q2->w, q1->v.z).val;
}

void quaternion_add(const quaternion_t *q1, const quaternion_t *q2, quaternion_t *out)
{
	vector3_add(&q1->v, &q2->v, &out->v);
	out->w.val = q1->w.val + q2->w.val;
}
void quaternion_sub(const quaternion_t *q1, const quaternion_t *q2, quaternion_t *out)
{
	vector3_sub(&q1->v, &q2->v, &out->v);
	out->w.val = q1->w.val - q2->w.val;
}

void quaternion_square(const quaternion_t *q, quaternion_t *out)
{
	out->w.val = fixed_mul(q->w, q->w).val - vector3_dot(&q->v, &q->v).val;
	out->v.x.val = fixed_mul(q->w, q->v.x).val << 1;
	out->v.y.val = fixed_mul(q->w, q->v.y).val << 1;
	out->v.z.val = fixed_mul(q->w, q->v.z).val << 1;
}

void quaternion_to_matrix(const quaternion_t *q, matrix_t *M)
{
	fixed_t x2 = fixed_mul(q->v.x, q->v.x).val;
	fixed_t y2 = fixed_mul(q->v.y, q->v.y).val;
	fixed_t z2 = fixed_mul(q->v.z, q->v.z).val;
	fixed_t xy = fixed_mul(q->v.x, q->v.y).val;
	fixed_t xz = fixed_mul(q->v.x, q->v.z).val;
	fixed_t yz = fixed_mul(q->v.y, q->v.z).val;
	fixed_t wx = fixed_mul(q->w, q->v.x).val;
	fixed_t wy = fixed_mul(q->w, q->v.y).val;
	fixed_t wz = fixed_mul(q->w, q->v.z).val;

	M->m11.val = FIXED_ONE.val - ((y2.val + z2.val) << 1);
	M->m12.val = (xy.val - wz.val) << 1;
	M->m13.val = (xz.val + wy.val) << 1;

	M->m21.val = (xy.val + wz.val) << 1;
	M->m22.val = FIXED_ONE.val - ((x2.val + z2.val) << 1);
	M->m23.val = (yz.val - wx.val) << 1;

	M->m31.val = (xz.val - wy.val) << 1;
	M->m32.val = (yz.val + wx.val) << 1;
	M->m33.val = FIXED_ONE.val - ((x2.val + y2.val) << 1);
}

void quaternion_rotate(const quaternion_t *q, const vector3_t *v, vector3_t *out)
{
	fixed_t x2 = fixed_mul(q->v.x, q->v.x).val;
	fixed_t y2 = fixed_mul(q->v.y, q->v.y).val;
	fixed_t z2 = fixed_mul(q->v.z, q->v.z).val;
	fixed_t xy = fixed_mul(q->v.x, q->v.y).val;
	fixed_t xz = fixed_mul(q->v.x, q->v.z).val;
	fixed_t yz = fixed_mul(q->v.y, q->v.z).val;
	fixed_t wx = fixed_mul(q->w, q->v.x).val;
	fixed_t wy = fixed_mul(q->w, q->v.y).val;
	fixed_t wz = fixed_mul(q->w, q->v.z).val;

	out->x.val = v->x.val
		- ((fixed_mul(fixed_add(y2, z2), v->x).val
			- fixed_mul(fixed_sub(xy, wz), v->y).val
			- fixed_mul(fixed_add(xz, wy), v->z).val) << 1);
	out->y.val = v->y.val
		- ((fixed_mul(fixed_add(x2, z2), v->y).val
			- fixed_mul(fixed_add(xy, wz), v->x).val
			- fixed_mul(fixed_sub(yz, wx), v->z).val) << 1);
	out->z.val = v->z.val
		- ((fixed_mul(fixed_add(x2, y2), v->z).val
			- fixed_mul(fixed_sub(xz, wy), v->x).val
			- fixed_mul(fixed_add(yz, wx), v->y).val) << 1);
}

void quaternion_rotate_inv(const quaternion_t *q, const vector3_t *v, vector3_t *out)
{
	fixed_t x2 = fixed_mul(q->v.x, q->v.x).val;
	fixed_t y2 = fixed_mul(q->v.y, q->v.y).val;
	fixed_t z2 = fixed_mul(q->v.z, q->v.z).val;
	fixed_t xy = fixed_mul(q->v.x, q->v.y).val;
	fixed_t xz = fixed_mul(q->v.x, q->v.z).val;
	fixed_t yz = fixed_mul(q->v.y, q->v.z).val;
	fixed_t wx = fixed_mul(q->w, q->v.x).val;
	fixed_t wy = fixed_mul(q->w, q->v.y).val;
	fixed_t wz = fixed_mul(q->w, q->v.z).val;

	out->x.val = v->x.val
		- ((fixed_mul(fixed_add(y2, z2), v->x).val
			- fixed_mul(fixed_add(xy, wz), v->y).val
			- fixed_mul(fixed_sub(xz, wy), v->z).val) << 1);
	out->y.val = v->y.val
		- ((fixed_mul(fixed_add(x2, z2), v->y).val
			- fixed_mul(fixed_sub(xy, wz), v->x).val
			- fixed_mul(fixed_add(yz, wx), v->z).val) << 1);
	out->z.val = v->z.val
		- ((fixed_mul(fixed_add(x2, y2), v->z).val
			- fixed_mul(fixed_add(xz, wy), v->x).val
			- fixed_mul(fixed_sub(yz, wx), v->y).val) << 1);
}

// Let q = [w, v] = [w, [x, y, z]].
// out = exp(w) * [cos(|v|), v * sin(|v|)]
void quaternion_exp(const quaternion_t *q, quaternion_t *out)
{
	fixed_t len = vector3_length(&q->v);
	fixed_t exp_w = fixed_exp(q->w);
	fixed_t sin_len, cos_len;
	fixed_sin_cos(len, &sin_len, &cos_len);
	sin_len = fixed_mul(exp_w, sin_len);

	out->w = fixed_mul(exp_w, cos_len);
	out->v.x = fixed_mul(fixed_div(q->v.x, len), sin_len);
	out->v.y = fixed_mul(fixed_div(q->v.y, len), sin_len);
	out->v.z = fixed_mul(fixed_div(q->v.z, len), sin_len);
}
